#include "BasicType/Quaternion.h"

namespace GCRE {
	namespace BasicType {


		Quaternion::Quaternion()
		{
			vec = _mm_set_ps1( 0.0F );
		}

		Quaternion::Quaternion( const Quaternion& q )
		{
			vec = q.vec;
		}

		Quaternion::Quaternion( SFFloat x, SFFloat y, SFFloat z, SFFloat w )
		{
			vec = _mm_set_ps( w, z, y, x );
		}

		Quaternion& Quaternion::operator=( const Quaternion& q )
		{
			vec = q.vec;
			return *this;
		}

		Quaternion Quaternion::operator+( const Quaternion& q ) const
		{
			Quaternion result;
			result.vec = _mm_add_ps( vec, q.vec );
			return result;
		}

		Quaternion Quaternion::operator-( const Quaternion& q ) const
		{
			Quaternion result;
			result.vec = _mm_sub_ps( vec, q.vec );
			return result;
		}

		Quaternion Quaternion::operator*( const Quaternion& q ) const
		{
			/************************************************************************/
			/*				q0 = x0 * i + y0 * j + z0 * k + w0;                                        */
			/*				q1 = x1 * i + y1 * j + z1 * k + w1;                                        */
			/*				q0 * q1 = ( w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1 ) * i +       */
			/*				                ( w0 * y1 - x0 * z1 + y0 * w1 + z0 * x1 ) * j +       */
			/*				                ( w0 * z1 + x0 * y1 - y0 * x1 + z0 * w1 ) * k +      */
			/*				                ( w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1 )                */
			/************************************************************************/
			Quaternion result;
			
			__m128 v0 = _mm_shuffle_ps( vec, vec, _MM_SHUFFLE( 3, 3, 3, 3 ) );
			__m128 v1 = q.vec;
			result.vec = _mm_mul_ps( v0, v1 );

			v0 = _mm_set_ps( -data[0], data[0], -data[0], data[0] );
			v1 = _mm_shuffle_ps( q.vec, q.vec, _MM_SHUFFLE( 0, 1, 2, 3 ) );
			v0 = _mm_mul_ps( v0, v1 );
			result.vec = _mm_add_ps( result.vec, v0 );
			
			v0 = _mm_set_ps( -data[1], -data[1], data[1], data[1] );
			v1 = _mm_shuffle_ps( q.vec, q.vec, _MM_SHUFFLE( 1, 0, 3, 2 ) );
			v0 = _mm_mul_ps( v0, v1 );
			result.vec = _mm_add_ps( result.vec, v0 );

			v0 = _mm_set_ps( -data[2], data[2], data[2], -data[2] );
			v1 = _mm_shuffle_ps( q.vec, q.vec, _MM_SHUFFLE( 2, 3, 0, 1 ) );
			v0 = _mm_mul_ps( v0, v1 );
			result.vec = _mm_add_ps( result.vec, v0 );

			return result;
		}

		Quaternion Quaternion::operator*( SFFloat scalar ) const
		{
			Quaternion result;
			__m128 r = _mm_set_ps1( scalar );
			result.vec = _mm_mul_ps( vec, r );
			return result;
		}

		Quaternion& Quaternion::operator+=( const Quaternion& q )
		{
			vec = _mm_add_ps( vec, q.vec );
			return *this;
		}

		Quaternion& Quaternion::operator-=( const Quaternion& q )
		{
			vec = _mm_sub_ps( vec, q.vec );
			return *this;
		}

		Quaternion& Quaternion::operator*=( const Quaternion& q )
		{
			*this = *this * q;
			return *this;
		}

		Quaternion& Quaternion::operator*=( SFFloat scalar )
		{
			__m128 r = _mm_set_ps1( scalar );
			vec = _mm_mul_ps( vec, r );
			return *this;
		}

		void Quaternion::Set( SFFloat x, SFFloat y, SFFloat z, SFFloat w )
		{
			vec = _mm_set_ps( w, z, y, x );
		}

		SFFloat Quaternion::Norm() const
		{
			SFFloat result;
			__m128 r0 = _mm_mul_ps( vec, vec );
			__m128 r1 = _mm_shuffle_ps( r0, r0, _MM_SHUFFLE( 3, 2, 3, 2 ) );
			r0 = _mm_add_ps( r0, r1 );
			r1 = _mm_shuffle_ps( r0, r0, _MM_SHUFFLE( 3, 2, 1, 1 ) );
			r0 = _mm_add_ss( r0, r1 );
			_mm_store_ss( &result, r0 );
			return result;
		}

		Quaternion Quaternion::operator/( SFFloat scalar ) const
		{
			Quaternion result;
			__m128 r = _mm_set_ps1( scalar );
			result.vec = _mm_div_ps( vec, r );
			return result;
		}

		Quaternion& Quaternion::operator/=( SFFloat scalar )
		{
			__m128 r = _mm_set_ps1( scalar );
			vec = _mm_div_ps( vec, r );
			return *this;
		}

		Quaternion Quaternion::operator-() const
		{
			Quaternion result;
			__m128 zero = _mm_set_ps1( 0.0F );
			result.vec = _mm_sub_ps( zero, vec );
			return result;
		}

		Quaternion Quaternion::Conjugate() const
		{
			Quaternion result( -data[0], -data[1], -data[2], data[3] );
			return result;
		}

		Quaternion Quaternion::Inverse() const
		{
			Quaternion result = Conjugate() / Norm();
			return result;
		}

		Quaternion Quaternion::Log() const
		{
			Quaternion result;		//	--TODO
			return result;
		}

		Quaternion Quaternion::Exp() const
		{
			Quaternion result;		//	--TODO
			return result;
		}

		void* Quaternion::operator new( size_t size )
		{
			return _mm_malloc( size, 16 );
		}

		void* Quaternion::operator new[]( size_t size )
		{
			return _mm_malloc( size, 16 );
		}

		void	Quaternion::operator delete[]( void *mem )
		{
			_mm_free( mem );
		}

		void	Quaternion::operator delete( void *mem )
		{
			_mm_free( mem );
		}

	}
}
